/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
#include "ano_cmd.h"
#include "drv_pwm.h"
#include "fc_state.h"
#include "rtthread.h"
#include "motor_mix.h"
#include "attitude.h"
#include "rc_controls.h"
#include "maths.h"
#include "attitude.h"
#include "imu.h"
#include "anoPTv8_report.h"

#define DBG_TAG "mix"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

motorMixer_t currentMixer; // 当前混控器
int16_t fc_motor[4] = {0};
int16_t motor[4] = {0};
static bool hw_motor_on_flag = false;
static float motorMixRange;
static int16_t motor_max;
static int16_t motor_min;
float throttle = 0;
bool airmodeEnabled = false;
int8_t motorOutputMixSign = 1;
int16_t motorOutputRange = 1000;
int16_t motorOutputMin = 0;
int16_t motorRangeMax = 1000;
int16_t motorRangeMin = 0;
motorMixer_t activeMixer[4] = {
    /**
     * @brief
     * M1  M4
     *   \/
     *   /\
     * M2  M3
     */
    {1.0f, 1.0f, -1.0f, 1.0f}, // FRONT_L
    {1.0f, 1.0f, 1.0f, -1.0f},   // REAR_L
    {1.0f, -1.0f, 1.0f, 1.0f}, // REAR_R
    {1.0f, -1.0f, -1.0f, -1.0f}, // FRONT_R
};
/**
 * @brief Get the throttle object
 *
 * @return float [0,1]
 */
float get_throttle(void)
{
    return throttle;
}
float getMotorMixRange(void)
{
    return motorMixRange;
}

static void applyMixerAdjustment(float *motorMix, const float motorMixMin, const float motorMixMax, const bool airmodeEnabled)
{
    if (motorMixRange > 1.0f)
    {
        for (int i = 0; i < 4; i++)
        {
            motorMix[i] /= motorMixRange;
        }
        // Get the maximum correction by setting offset to center when airmode enabled
        if (airmodeEnabled)
        {
            throttle = 0.5f;
        }
    }
    else
    {
        if (airmodeEnabled || throttle > 0.5f)
        {
            throttle = constrainf(throttle, -motorMixMin, 1.0f - motorMixMax);
        }
    }
}

void motor_mix(void)
{
    if (is_ano_motor_on() || FC_STATE_CHECK(FC_ARMED) || FC_STATE_CHECK(FC_OK_TO_ARM) || FC_STATE_CHECK(FC_ARMING_ALERK))
    {
        if (!hw_motor_on_flag)
        {
            // clear motor value before motor on, avoid motor jitter
            fc_motor[0] = 0;
            fc_motor[1] = 0;
            fc_motor[2] = 0;
            fc_motor[3] = 0;
            ano_motor_value[0] = 0;
            ano_motor_value[1] = 0;
            ano_motor_value[2] = 0;
            ano_motor_value[3] = 0;

            motor_on();
            hw_motor_on_flag = true;
        }
    }
    else
    {
        if (hw_motor_on_flag)
        {
            fc_motor[0] = 0;
            fc_motor[1] = 0;
            fc_motor[2] = 0;
            fc_motor[3] = 0;
            ano_motor_value[0] = 0;
            ano_motor_value[1] = 0;
            ano_motor_value[2] = 0;
            ano_motor_value[3] = 0;

            hw_motor_on_flag = false;
            throttle = 0.f;
            /**clear motor output */
            motors_set(0, 0, 0, 0);
            motor_off();
        }
    }

    if (hw_motor_on_flag)
    {
        motor_max = 0;
        motor_min = 0;

        // Calculate and Limit the PID sum
        const float scaledAxisPidRoll =
            constrainf(pidData[ROLL].Sum, -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING;
        const float scaledAxisPidPitch =
            constrainf(pidData[PITCH].Sum, -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING;

        uint16_t yawPidSumLimit = currentPidProfile->pidSumLimitYaw;
        float scaledAxisPidYaw =
            constrainf(pidData[YAW].Sum, -yawPidSumLimit, yawPidSumLimit) / PID_MIXER_SCALING;

        throttle = currentMixer.throttle / 1000.f;
        // use scaled throttle, without dynamic idle throttle offset, as the input to antigravity
        pidUpdateAntiGravityThrottleFilter(throttle);

        float motorMix[4];
        float motorMixMax = 0, motorMixMin = 0;
        for (int8_t i = 0; i < 4; i++)
        {
            float mix =
                scaledAxisPidRoll * activeMixer[i].roll +
                scaledAxisPidPitch * activeMixer[i].pitch +
                scaledAxisPidYaw * activeMixer[i].yaw;

            if (mix > motorMixMax)
            {
                motorMixMax = mix;
            }
            else if (mix < motorMixMin)
            {
                motorMixMin = mix;
            }
            motorMix[i] = mix;
        }
        motorMixRange = motorMixMax - motorMixMin;
        applyMixerAdjustment(motorMix, motorMixMin, motorMixMax, airmodeEnabled);

        for (int i = 0; i < 4; i++)
        {
            float motorOutput = motorOutputMixSign * motorMix[i] + throttle * activeMixer[i].throttle;
            motorOutput = motorOutputMin + motorOutputRange * motorOutput;
            motorOutput = constrain(motorOutput, motorRangeMin, motorRangeMax);
            if (FC_STATE_CHECK(FC_ARMED))
            {
                fc_motor[i] = motorOutput;
            }
            else
            {
                fc_motor[i] = currentMixer.throttle;
            }
            motor[i] = fc_motor[i] + ano_motor_value[i];
        }
        motors_set(motor[3], motor[2], motor[1], motor[0]);
    }

}
